/*******************************************************************************
 * Copyright (C) 2013 Jeremy Brodt and Kevin Gillespie
 * Description: Position Controller
 *         $Id: pos.c 24 2014-04-13 21:56:58Z brodtj1212@gmail.com $
 *******************************************************************************
 */

#include <stdio.h>
#include <string.h>
#include "maxq_config.h"
#include "comm/maxq_comm.h"
#include "car.h"
#include "pos.h"
#include "encoder.h"
#include "trig.h"
#include "compass.h"
#include "wall.h"

/***** Definitions *****/
//#define USE_COMPASS

#define MAX_SETTING   (30 << 4)

#define TEN_DEGREES   45

//#define EXT_LOG
#ifdef EXT_LOG
#define POS_LOG_SIZE  20
#else
#define POS_LOG_SIZE  50
#endif

typedef struct {
  int32_t odo[NUM_WHEELS];
#ifdef EXT_LOG
  int16_t dU_left_mms7;
  int16_t dU_right_mms7;
  int16_t dU_center_mms7;
  int32_t dx_mms7;
  int32_t dy_mms7;
  int16_t dHeading;
#endif
  int32_t X;
  int32_t Y;
  uint16_t Heading;
  int16_t wall[NUM_WHEELS];
} pos_log_t;

/***** Global Data *****/
int32_t X, Y;
uint16_t Heading;
int32_t last_odo_left, last_odo_right;
pos_log_t pos_log[POS_LOG_SIZE];
unsigned int pos_logi;
uint16_t compass_offset;

// Heading PID
int heading_error;
static int hd_prev_setting;
static int hd_prev_error;
static int hd_prev_error_valid;
static int hd_cumulative_error;

// Lateral PID
int lateral_error;
static int lt_prev_error;
static int lt_prev_error_valid;
static int lt_cumulative_error;

/***** Function Prototypes *****/
uint16_t angle_add(int16_t a, int16_t b);
int16_t ssadd16(int16_t a, int16_t b);

/******************************************************************************/
void pos_init(void)
{
  X = 0;
  Y = 0;
  Heading = 0;
  last_odo_left = 0;
  last_odo_right = 0;
  memset(pos_log, 0, sizeof(pos_log));
  pos_logi = 0;
#ifdef USE_COMPASS
  do {
    compass_task();
    compass_offset = deg10_to_rads8(compass_heading);
  } while (compass_offset > RADS8_MAX);
#endif
  pos_ctrl_clear();
}

/******************************************************************************/
int pos_task(void)
{
  int16_t dU_left_mms7, dU_right_mms7, dU_center_mms7;
  int16_t dHeading_s8;
  int32_t dx_mms7, dy_mms7;
  int16_t sin_s8, cos_s8;
#ifdef USE_COMPASS
  int16_t temp;
#endif

  dU_left_mms7 = (odometer[LEFT] - last_odo_left) * mms7_per_cnt[LEFT];
  dU_right_mms7 = (odometer[RIGHT] - last_odo_right) * mms7_per_cnt[RIGHT];

  dU_center_mms7 = (dU_right_mms7 + dU_left_mms7 + 1) >> 1;

#ifdef USE_COMPASS
  dHeading_s8 = 0;
  temp = deg10_to_rads8(compass_heading) - compass_offset;
  if (temp < 0)
    temp += RADS8_MAX;
  Heading = temp;
#else
  dHeading_s8 = (((int32_t)(dU_left_mms7 - dU_right_mms7) << 1) + (WHEELBASE_MM/2)) / WHEELBASE_MM;
  Heading = angle_add(Heading, dHeading_s8);
#endif

  sin_s8 = sin_s8_table[Heading];
  cos_s8 = cos_s8_table[Heading];

  dx_mms7 = ((int32_t)dU_center_mms7 * sin_s8) >> 8;
  dy_mms7 = ((int32_t)dU_center_mms7 * cos_s8) >> 8;

  X += dx_mms7 >> 7;
  Y += dy_mms7 >> 7;

  last_odo_left = odometer[LEFT];
  last_odo_right = odometer[RIGHT];

  if (pos_logi < POS_LOG_SIZE) {
    if ( (pos_logi == 0) || (odometer[LEFT] != 0) || (odometer[RIGHT] != 0) ) {
  
      pos_log[pos_logi].odo[LEFT] = odometer[LEFT];
      pos_log[pos_logi].odo[RIGHT] = odometer[RIGHT];

#ifdef EXT_LOG
      pos_log[pos_logi].dHeading = dHeading_s8;

      pos_log[pos_logi].dU_left_mms7 = dU_left_mms7;
      pos_log[pos_logi].dU_right_mms7 = dU_right_mms7;
      pos_log[pos_logi].dU_center_mms7 = dU_center_mms7;

      pos_log[pos_logi].dx_mms7 = dx_mms7;
      pos_log[pos_logi].dy_mms7 = dy_mms7;
#endif

      pos_log[pos_logi].Heading = Heading;
      pos_log[pos_logi].X = X;
      pos_log[pos_logi].Y = Y;
  
      pos_log[pos_logi].wall[LEFT] = wall_dist[LEFT];
      pos_log[pos_logi].wall[RIGHT] = wall_dist[RIGHT];

      pos_logi++;
    }
  }

  return 1;
}

/******************************************************************************/
void pos_ctrl_clear(void)
{
  hd_prev_setting = 0;
  hd_prev_error = 0;
  hd_prev_error_valid = 0;
  hd_cumulative_error = 0;
  lt_prev_error = 0;
  lt_prev_error_valid = 0;
  lt_cumulative_error = 0;
  heading_error = 0;
  lateral_error = 0;
}

/******************************************************************************/
int heading_pid(void)
{
  int derivative_error;
  int new_setting;
  int ekp = 0;
  int eki = 0;
  int ekd = 0;

  // Calculate the derivative error
  if (hd_prev_error_valid)
    derivative_error = heading_error - hd_prev_error;
  else
    derivative_error = 0;

  // Accumulate integral error
  hd_cumulative_error += heading_error;

  ekp = heading_error / 2;
//  eki = cumulative_error / 256;
  ekd = derivative_error / 10;

  // Calculate the new controller output
  new_setting = ssadd16(hd_prev_setting, ekd);
  new_setting = ssadd16(new_setting, eki);
  new_setting = ssadd16(new_setting, ekp);

  // Saturate setting output
  if (new_setting > MAX_SETTING)
    new_setting = MAX_SETTING;
  else if (new_setting < -MAX_SETTING)
    new_setting = -MAX_SETTING;

  // Record state for next iteration
  hd_prev_setting = new_setting;
  hd_prev_error = heading_error;
  hd_prev_error_valid = 1;

  return (new_setting >> 4);
}

/******************************************************************************/
int lateral_pid(void)
{
  int derivative_error;
  int new_setting;
  int ekp = 0;
  int ekd = 0;

  // Calculate the derivative error
  if (lt_prev_error_valid)
    derivative_error = lateral_error - lt_prev_error;
  else
    derivative_error = 0;

  // Accumulate integral error
  lt_cumulative_error += lateral_error;

  ekp = lateral_error << 3;
  ekd = derivative_error << 8;

  // Calculate the new controller output
  new_setting = ekd;
  new_setting = ssadd16(new_setting, ekp);

  // Saturate setting output
  if (new_setting > MAX_SETTING)
    new_setting = MAX_SETTING;
  else if (new_setting < -MAX_SETTING)
    new_setting = -MAX_SETTING;

  // Record state for next iteration
  lt_prev_error = lateral_error;
  lt_prev_error_valid = 1;

  return (new_setting >> 4);
}

/******************************************************************************/
int pos_ctrl(int desired_heading, int desired_left, int desired_right)
{
  int new_setting;

  // Calculate our heading error
  heading_error = Heading - desired_heading;
  if (heading_error > (HEADING_MAX >> 1)) {
    heading_error -= HEADING_MAX;
  } else if (heading_error < -(HEADING_MAX >> 1)) {
    heading_error += HEADING_MAX;
  }

  // Calculate our lateral error
  if ( (wall_dist[LEFT] >= 0) && (desired_left >= 0) && 
       (wall_dist[RIGHT] >= 0) && (desired_right >= 0) ) {
    lateral_error = (wall_dist[LEFT] - desired_left) + (desired_right - wall_dist[RIGHT]);
  } else if ( (wall_dist[LEFT] >= 0) && (desired_left >= 0) ) {
    lateral_error = (wall_dist[LEFT] - desired_left) << 1;
  } else if ( (wall_dist[RIGHT] >= 0) && (desired_right >= 0) ) {
    lateral_error = (desired_right - wall_dist[RIGHT]) << 1;
  } else {
    lateral_error = 0;
  }

#if 0
  // If we are within our heading window, adjust for lateral position
  // If we are within lateral position, adjust for heading
  if ( (heading_error > -TEN_DEGREES) && (heading_error < TEN_DEGREES) ) {
    new_setting = lateral_pid();
  }
  else {
    new_setting = heading_pid();
  }
#else
  new_setting = lateral_pid();
#endif

  return new_setting;
}

/******************************************************************************/
void pos_log_display(void)
{
  unsigned int i;

  maxq_printf(C"\r\n***** Position Log *****\r\n");
#ifdef EXT_LOG
  maxq_printf(C"Idx   CntsL  CntsR   OdoL   OdoR    dUL    dUR    dUC dHead Head     dx     dy      X      Y\r\n");
#else
  maxq_printf(C"Idx   CntsL  CntsR   OdoL   OdoR Head      X      Y\r\n");
#endif
  for (i = 0; i < pos_logi; i++) {
    maxq_printf(C"%3u ", i);
    print_i32(pos_log[i].odo[LEFT]);
    print_i32(pos_log[i].odo[RIGHT]);
    print_i32(calc_odometer_value(LEFT, pos_log[i].odo[LEFT]));
    print_i32(calc_odometer_value(RIGHT, pos_log[i].odo[RIGHT]));
#ifdef EXT_LOG
    maxq_printf(C" %6d %6d %6d", pos_log[i].dU_left_mms7, pos_log[i].dU_right_mms7, pos_log[i].dU_center_mms7);
    maxq_printf(C"  %4d", rads8_to_deg(pos_log[i].dHeading));
#endif
    maxq_printf(C" %4d", rads8_to_deg(pos_log[i].Heading));
#ifdef EXT_LOG
    print_i32(pos_log[i].dx_mms7);
    print_i32(pos_log[i].dy_mms7);
#endif
    print_i32(pos_log[i].X);
    print_i32(pos_log[i].Y);
    maxq_printf(C" %3d %3d", pos_log[i].wall[LEFT], pos_log[i].wall[RIGHT]);
    maxq_printf(C"\r\n");
  }
}

/******************************************************************************/
uint16_t angle_add(int16_t a, int16_t b)
{
  int16_t sum;

  sum = a + b;

  // 2 * PI * 2^8 = 1608.5
  if (sum > 1609) {
    sum -= 1609;
  }
  else if (sum < 0) {
    sum += 1609;
  }

  return sum;
}

/******************************************************************************/
int16_t ssadd16(int16_t a, int16_t b)
{
  int sum;

  sum = a + b;

  if ( (a > 0) && (b > 0) && (sum < 0) )
    sum = 0x7FFF;
  else if ( (a < 0) && (b < 0) && (sum > 0) )
    sum = 0x8000;

  return sum;
}
